#include "behavior.h"
#include "math.h"
#include "kuruma.h"
static PIDController angle_pid;
static PIDController arc_pid;
void behavior_init(void) {
    // 角度 PID 初始化（控制目标：朝向角 θ）
    pid_init(&angle_pid,
             3.0f,     // Kp：角度误差 → 角速度输出
             0.1f,     // Ki：消除静差（小一点，避免震荡）
             0.3f,     // Kd：抑制过冲
             0.005f,   // dt：5ms 控制周期
             2.0f,     // integral_max：积分上限（rad/s）
             -2.0f,    // integral_min
             2.0f,     // output_max：最大角速度（rad/s），约 114°/s
             -2.0f    // output_min
    );
	
	pid_init(&arc_pid,
			2.0f,     // Kp：角度误差 → 角速度输出
             0.1f,     // Ki：消除静差（小一点，避免震荡）
             0.001f,     // Kd：抑制过冲
             0.005f,   // dt：5ms 控制周期
             2.0f,     // integral_max：积分上限（rad/s）
             -2.0f,    // integral_min
             2.0f,     // output_max：最大角速度（rad/s），约 114°/s
             -2.0f    // output_min
    );

    // 启用目标斜坡（平滑启动，避免突加目标导致过冲）
    pid_enable_ramp(&angle_pid, 1);
    pid_set_ramp_step(&angle_pid, 0.5f);  // 每次目标增加 0.5 rad/s
	
	pid_enable_ramp(&arc_pid, 1);
	pid_set_ramp_step(&arc_pid, 0.5f);

    // 积分突释抑制（防止目标突变时积分过大）
    angle_pid.integral_damping_ratio = 0.3f;  // 典型值 0.3
}
/**
 * @brief 前进或后退指定距离
 * @param distance 目标距离（米），>0 为前进，<0 为后退
 * @param speed 行驶速度（m/s），必须为正数（函数内部自动处理方向）
 * @return 1=完成，0=进行中
 */
int drive_straight(float distance, float _speed) {
    static float target_distance = 0.0f;
	static float speed;
    static int started = 0;
	static int forward;

    if (!started) {
		forward = distance >= 0;
        target_distance = distance + odometry_get_distance();
		speed = _speed;
        started = 1;
    }
	
	// 确保 speed 为正（作为速率使用）
    speed = fabsf(speed);

    // 判断是否完成：根据 distance 正负，traveled 要趋近于 distance
    int is_complete = 0;
    float remaining = target_distance - odometry_get_distance();
	
	is_complete = fabsf(remaining) <= 0.005f;

    if (is_complete) {
        kuruma_set_velocity(0, 0);
        started = 0;
		speed = 0;
		forward = 0;
        return 1;
    } else {
        // 根据 remaining 符号决定运动方向
        float linear_velocity = (forward) ? speed : -speed;
        kuruma_set_velocity(linear_velocity, 0);
        return 0;
    }
}

/**
 * @brief 转向指定绝对角度（角度制）
 * @param target_angle_deg 目标朝向（角度，-180 ~ 180）
 * @return 1=完成，0=进行中
 */
int turn_to_angle(float target_angle_deg) {
    static int started = 0;
    static float target_rad = 0;

    if (!started) {
        float current_rad = odom.theta;
        target_rad = target_angle_deg * DEG_TO_RAD;

        // === 确保目标角和当前角度的差值落在 [-π, π] ===
        float error = target_rad - current_rad;
        while (error > M_PI) {
            target_rad -= 2 * M_PI;
            error -= 2 * M_PI;
        }
        while (error < -M_PI) {
            target_rad += 2 * M_PI;
            error += 2 * M_PI;
        }

        pid_set_target(&angle_pid, target_rad);
        started = 1;
    }

    float current_rad = odom.theta;
    float angular_vel = pid_update(&angle_pid, current_rad);
    float error_deg = angle_pid.error * RAD_TO_DEG;  // 由 pid 内部算出 error

    if (fabsf(error_deg) < 2.0f && fabsf(angular_vel) < 0.1f) {
        kuruma_set_velocity(0, 0);
        started = 0;
        return 1;
    } else {
        kuruma_set_velocity(0, angular_vel);
        return 0;
    }
}

/**
 * @brief 相对旋转指定角度
 * @param degrees 旋转角度（+为逆时针，-为顺时针，单位：度）
 * @param max_angular_speed 最大角速度（单位：度/秒）
 * @return 1=完成，0=进行中
 */
int turn_by_degree(float degrees, float max_angular_speed) {
    static float target_angle_rad = 0.0f;
    static int started = 0;

    // 将最大角速度从 °/s 转换为 rad/s
    float max_angular_speed_rad = max_angular_speed * DEG_TO_RAD;

    if (!started) {
        float current_rad = odom.theta;
        target_angle_rad = current_rad + degrees * DEG_TO_RAD;

        // === 关键：归一化到最近等价角度 ===
        target_angle_rad = wrap_angle(target_angle_rad);

        // 确保目标和当前之间的差值落在 [-π, π]
        float error = target_angle_rad - current_rad;
        while (error > M_PI) {
            target_angle_rad -= 2 * M_PI;
            error -= 2 * M_PI;
        }
        while (error < -M_PI) {
            target_angle_rad += 2 * M_PI;
            error += 2 * M_PI;
        }

        pid_set_target(&angle_pid, target_angle_rad);
        started = 1;
    }

    float current_rad = odom.theta;
    float angular_vel = pid_update(&angle_pid, current_rad);
    float error_deg = angle_pid.error * RAD_TO_DEG;

    if (fabsf(error_deg) < 2.0f && fabsf(angular_vel) < 0.1f) {
        kuruma_set_velocity(0, 0);
        started = 0;
        return 1;
    } else {
        // 限制输出角速度（先限制再设置）
        angular_vel = fmaxf(-max_angular_speed_rad, fminf(max_angular_speed_rad, angular_vel));
        kuruma_set_velocity(0, angular_vel);
        return 0;
    }
}


/**
 * @brief 沿指定半径的圆弧行驶指定角度（纯 PID 控制，仅限最大角速度）
 * @param radius 转弯半径（米），>0 左转（逆时针），<0 右转（顺时针）
 * @param angle_deg 目标转过的角度（度），+为逆时针，-为顺时针
 * @param max_angular_speed_deg 最大允许角速度（度/秒）
 * @return 1=完成，0=进行中
 *
 * @note 线速度由 v = ω * |radius| 自动决定，不单独设置
 */
int drive_arc_by_angle_pid(float radius, float angle_deg, float max_angular_speed_deg) {
    static float start_theta = 0.0f;        // 起始朝向
    static float target_d_theta = 0.0f;     // 目标相对转角 (rad)
    static int started = 0;

    const float ANGLE_TOLERANCE_DEG = 1.5f;         // 完成角度误差容限
    const float MIN_ANGULAR_VEL = 0.05f;            // 停止时最小角速度 (rad/s)

    // 单位转换
    float max_angular_speed_rad = max_angular_speed_deg * DEG_TO_RAD;
    max_angular_speed_rad = fminf(max_angular_speed_rad, 3.0f); // 安全上限 ~172°/s

    // 安全：最小转弯半径
    if (fabsf(radius) < 0.05f) {
        radius = (radius >= 0) ? 0.05f : -0.05f;
    }

    if (!started) {
        start_theta = odom.theta;
        target_d_theta = angle_deg * DEG_TO_RAD;
        pid_set_target(&arc_pid, target_d_theta);   // 设定 PID 目标：累计转角
        started = 1;
    }

    // 当前已转过的角度（相对于起始方向）
    float d_theta = wrap_angle(odom.theta - start_theta);

    // 计算误差（最短路径）
    float error = wrap_angle(target_d_theta - d_theta);

    // 使用 PID 计算所需角速度（输出为 rad/s）
    float angular_vel = pid_update(&arc_pid, d_theta);

    // 限制最大角速度（带方向）
    angular_vel = fmaxf(-max_angular_speed_rad, fminf(max_angular_speed_rad, angular_vel));

    // ✅ 关键：确保 angular_vel 的符号与转弯方向一致（不能反向）
    float expected_sign = (radius > 0) ? 1.0f : -1.0f;
    float desired_sign = (target_d_theta > 0) ? 1.0f : -1.0f;
    float sign = (expected_sign * desired_sign > 0) ? 1.0f : -1.0f;
    if (angular_vel * sign < 0) {
        angular_vel = 0.0f;
    }

    // ✅ 根据角速度和半径自动计算线速度：v = ω * r
    float linear_vel = angular_vel * radius;  // 注意：radius 可正可负，但 v 大小由 |ω|*|r| 决定

    // 判断是否完成：角度误差小 + 角速度低
    if (fabsf(error) < ANGLE_TOLERANCE_DEG * DEG_TO_RAD &&
        fabsf(angular_vel) < MIN_ANGULAR_VEL) {
        kuruma_set_velocity(0, 0);
        started = 0;
        return 1;
    } else {
        kuruma_set_velocity(linear_vel, angular_vel);
        return 0;
    }
}

/**
 * @brief 沿圆弧行驶指定角度
 * @param radius 转弯半径（米），>0 左转，<0 右转
 * @param angle_deg 转弯角度（度），+为逆时针
 * @param speed 线速度（m/s）
 * @return 1=完成，0=进行中
 */
int drive_arc_by_angle(float radius, float angle_deg, float speed) {
    static float total_rotated = 0.0f;   // 累计转过的角度（rad）
    static float last_theta = 0.0f;
    static int started = 0;

    // 安全：最小转弯半径
    if (fabs(radius) < 0.05f) {
        radius = (radius >= 0) ? 0.05f : -0.05f;
    }

    if (!started) {
        total_rotated = 0.0f;
        last_theta = odom.theta;
        started = 1;
        return 0;
    }

    // 获取当前角度
    float d_theta = wrap_angle(odom.theta - last_theta);
    total_rotated += d_theta;
    last_theta = odom.theta;

    float target_rad = angle_deg * DEG_TO_RAD;

    // 判断是否完成（考虑正负）
    if ((angle_deg > 0 && total_rotated >= target_rad - 0.05f) ||
        (angle_deg < 0 && total_rotated <= target_rad + 0.05f)) {
        kuruma_set_velocity(0, 0);
        started = 0;
        return 1;
    }

    // 设置速度
    float angular_vel = speed / radius;
    kuruma_set_velocity(speed, angular_vel);
    return 0;
}
/**
 * @brief 沿圆弧行驶
 * @param radius 转弯半径（米），>0 左转，<0 右转
 * @param distance 弧长（米），<0 表示持续运行
 * @param speed 线速度（m/s）
 * @return 1=完成，0=进行中
 */
int drive_arc(float radius, float distance, float speed) {
    static float start_distance = 0.0f;
    static int started = 0;

    // 安全检查
    if (fabs(radius) < 0.05f) {
        // 半径太小，近似直行
        radius = (radius >= 0) ? 0.05f : -0.05f;
    }

    if (!started) {
        start_distance = odometry_get_distance();
        started = 1;
    }

    // 计算当前已走弧长
    float traveled = odometry_get_distance() - start_distance;

    // 如果 distance < 0，表示持续运行（如绕圈）
    if (distance > 0 && traveled >= distance) {
        kuruma_set_velocity(0, 0);
        started = 0;
        return 1;  // 完成
    }

    // 计算所需角速度 w = v / R
    float angular_vel = speed / radius;

    // 设置速度
    kuruma_set_velocity(speed, angular_vel);

    return 0;  // 进行中
}

/**
 * @brief 原地旋转（持续行为，需手动停止）
 * @param angular_speed 角速度（rad/s，+为逆时针）
 * @return 0=持续中，1=可设计为“旋转n圈后完成”
 */
int rotate_in_place(float angular_speed) {
    kuruma_set_velocity(0, angular_speed);
    return 0;  // 永不自动完成，需外部调用 stop
}

/**
 * @brief 平滑减速停止
 * @return 1=已停止，0=正在减速
 */
int stop_smoothly() {
    static float current_speed = 0.0f;
    static int started = 0;

    if (!started) {
        current_speed = odom.linear_vel;
        started = 1;
    }

    // 简单线性减速
    current_speed -= 0.05f;  // 0.05 m/s² 减速度
    if (current_speed <= 0) {
        kuruma_set_velocity(0, 0);
        started = 0;
        return 1;
    } else {
        kuruma_set_velocity(current_speed, 0);
        return 0;
    }
}

/**
 * @brief 保持当前位置（需位置PID，可选）
 * @return 0=持续保持
 */
int hold_position() {
    // 可用 odometry.x/y 做 PID 反馈，防止漂移
    // 当前可简单设为零速度
    kuruma_set_velocity(0, 0);
    return 0;
}
